import parapin
from parapin.CONST import *



arm_edge = 260
scale_factor = 1



class EggBot:
  def __init__(self):
    # Open the parallel port and initialize the pins we will use.
    # NOTE: This requires administrator privelages.
    self.port = parapin.Port(LPT1, outmode=LP_DATA_PINS)
    self.d1 = self.port.get_pin(2)
    self.s1 = self.port.get_pin(3)
    self.d2 = self.port.get_pin(4)
    self.s2 = self.port.get_pin(5)
    self.d1.clear()
    self.d2.clear()
    self.s1.clear()
    self.s2.clear()

    # We keep track of the place we claim to be (arm, spin) and the place we actually are (arm_act, spin_act).
    self.arm = self.arm_act = 0.0
    self.spin = self.spin_act = 0.0

  def move(self, arm, spin):
    if arm == 0 and spin == 0:
      return

    arm, spin = scale_factor*arm, scale_factor*spin

    # Set the direction.
    self.d1.set() if spin < 0 else self.d1.clear()
    self.d2.set() if arm < 0 else self.d2.clear()

    r = 1 if arm*arm + spin*spin <= 1 else sqrt(arm*arm + spin*spin)
    a0 = self.arm
    s0 = self.spin

    # Draw a straight line from our current position to the new position.
    arm_end = self.arm + arm
    arm_increment = arm / r
    spin_end = self.spin + spin
    spin_increment = spin / r
    for i in range(1, ceil(r) + 1):
      arm_next = a0 + (min(i*arm_increment, arm) if arm >= 0 else max(i*arm_increment, arm))
      spin_next = s0 + (min(i*spin_increment, spin) if spin >= 0 else max(i*spin_increment, spin))
      print self.arm, self.spin, self.arm_act, self.spin_act, arm_next, spin_next
      if self.arm_act != round(arm_next) and self.spin_act != round(spin_next) and abs(round(arm_next)) <= arm_edge:
        (self.s1 | self.s2).pulse(0, 0, 5, 3)
        self.arm_act, self.spin_act = round(arm_next), round(spin_next)
      elif self.arm_act != round(arm_next) and abs(round(arm_next)) <= arm_edge:
        self.s2.pulse(0, 0, 5, 3)
        self.arm_act = round(arm_next)
      elif self.spin_act != round(spin_next):
        self.s1.pulse(0, 0, 5, 3)
        self.spin_act = round(spin_next)
      else:
        # This is just a nop.
        pass

      self.arm = arm_next
      self.spin = spin_next



  def move_absolute(self, arm, spin):
    # Use self.move to move us to the absolute position.
    self.move(arm - self.arm, spin - self.spin)

